# -*- coding: utf-8 -*-
# @Time    : 2024/11/18 16:04
# @Author  : sjh
# @Site    : 
# @File    : scoreAction.py
# @Comment :
import json
from utils.CalculateUtils import CalcuUtils as util
import numpy as np

from utils.genaral import plot_peak, plot_peak_1d, butterworth_filter_1d
###

class ActionScore():
    Const_KEY_POINTS = ['nose', 'left_eye', 'right_eye', 'left_ear', 'right_ear',
                        'right_ear', 'right_shoulder', 'left_elbow', 'right_elbow', 'left_wrist',
                        'right_wrist', 'left_hip', 'right_hip', 'left_knee', 'right_knee',
                        'left_ankle', 'right_ankle', 'head', 'neck', 'hip',
                        'left_big_toe', 'right_big_toe', 'left_small_toe', 'right_small_toe', 'left_heel',
                        'right_heel']
    def filter_keypoints(self, keypoints, key_remove):
        if key_remove == 'all':
            key_remove = list(range(26))
        filtered_keypoints = []
        self.ori_points_list = key_remove
        keypoints_names = [self.Const_KEY_POINTS[key] for key in key_remove]
        previous_frame = None  # 用于存储上一帧的值
        keypoints_num = len(key_remove)
        for d_idx, d in enumerate(keypoints):
            remove = False
            find_num = 0
            for key, value in enumerate(d):
                if key in key_remove:
                    value = list(value.values())[0]
                    if any(v == 0.0 for v in value):
                        if previous_frame is not None:
                            # 使用上一帧的值替换当前帧
                            d[key] = previous_frame[key]

                        find_num += 1
                        if find_num == keypoints_num:
                            break
                        # break

            filtered_keypoints.append(d)
            previous_frame = d  # 更新上一帧为当前帧

        return filtered_keypoints
    def initService(self,points_list, key_filter=False):
        self.points_list = points_list
        self.points_list = [d for d in points_list if d]
        # 使用过滤函数
        if key_filter:
            self.points_list = self.filter_keypoints(self.points_list, key_filter)
        return list(range(len(points_list)))

    def getLimbsPosition(self, keyPointInt, numFrame, ndim=3):

        if ndim==3:
            keyPointInt = int(keyPointInt)

            keypoint_name, keypoint_coords = list(self.points_list[numFrame][keyPointInt].items())[0]
            position=np.array(keypoint_coords) * 0.001
        return position

    def find_rising_points(self, data, peak_threshold=3):
        """
        找到每个峰值点之前的上升点（梯度由负变正的点）。

        Args:
            data (list or numpy.ndarray): 数据序列。

        Returns:
            list: 每个峰值点之前的上升点索引。
            list: 峰值点索引。
        """
        data = np.array(data)
        rising_points = []  # 上升点索引
        peaks = []  # 峰值点索引

        # 计算一阶差分
        diff = np.diff(data)

        # 遍历一阶差分，寻找峰值点和上升点
        for i in range(1, len(diff)):
            # 找到峰值点（梯度由正变负）
            if diff[i - 1] > 0 and diff[i] < 0 and data[i] >= peak_threshold:  # 添加峰值阈值过滤
                peaks.append(i)

                # 从峰值点向前找到最近的上升点（梯度由负变正）
                for j in range(i - 1, -1, -1):
                    if diff[j - 1] < 0 and diff[j] > 0:
                        rising_points.append(j)
                        break

        return rising_points, peaks
    def action_1_score(self, jsonPath):
        def calculate_speed_and_displacement_with_start_time(left_ankle_3d, right_ankle_3d, fps,neck, *points):
            """
            计算脚部关键点的速度和位移，并判断开始运动的时刻。

            Args:
                left_ankle_3d (numpy.ndarray): 左脚踝关键点，形状为 (frames, 3)。
                right_ankle_3d (numpy.ndarray): 右脚踝关键点，形状为 (frames, 3)。
                fps (int): 视频的帧率。
                speed_threshold (float): 判断开始运动的速度阈值。

            Returns:
                dict: 包含速度、位移、以及开始运动时刻的信息。
            """
            # 初始化结果
            left_speeds = []
            right_speeds = []
            left_displacements = []
            right_displacements = []

            start_frame = None
            frame_count = 0  # 计数器，用于连续帧计数
            frame_interval_threshold = 15  # 连续帧间隔的阈值
            z_threshold = 10  # z 轴差值的阈值
            # 遍历所有帧，计算速度和位移
            a = []
            neck_z_dis = []
            vector0 = left_ankle_3d[0] - right_ankle_3d[0]
            vector0 = util.vectorProjectionHorizontal(vector0)

            for i in range(len(left_ankle_3d)):
                # 找左右脚角度
                vector = left_ankle_3d[i]-right_ankle_3d[i]
                vector = util.vectorProjectionHorizontal(vector)
                angle = util.vectorAngle(vector0, vector)
                a.append(angle)
                # 计算左右脚与第一帧高度差
                left_displacements.append(abs(left_ankle_3d[i][1] - left_ankle_3d[1][1]))
                right_displacements.append(abs(right_ankle_3d[i][1] - right_ankle_3d[1][1]))

                # 计算膝踝脚夹角
                left_vector = util.vectorAngle(util.vectorProjectionHorizontal(points[2][i] - left_ankle_3d[i]), util.vectorProjectionHorizontal(points[0][i] - left_ankle_3d[i]))
                right_vector = util.vectorAngle(util.vectorProjectionHorizontal(points[3][i] - left_ankle_3d[i]), util.vectorProjectionHorizontal(points[0][i] - left_ankle_3d[i]))
                # 计算与第一帧的深度差值
                neck_z_dis.append(neck[i][2] - neck[0][2])

            a = butterworth_filter_1d(np.array(a))
            # 计算开始运动的时间（秒）
            start_time = start_frame / fps if start_frame is not None else None

            return {
                "left_speed": left_speeds,
                "right_speed": right_speeds,
                "left_displacement": left_displacements,
                "right_displacement": right_displacements,
                "start_frame": start_frame,
                "start_time": start_time,
                "a":a,
                "neck_z_dis":neck_z_dis
            }

        def is_walking(speed_data, displacement_data, speed_threshold=0.5, displacement_threshold=10):
            """
            判断是否开始走路。

            Args:
                speed_data (dict): 左右脚的速度数据。
                displacement_data (dict): 左右脚的位移数据。
                speed_threshold (float): 判断为运动的速度阈值。
                displacement_threshold (float): 判断为运动的位移阈值。

            Returns:
                bool: 是否开始走路。
            """
            # 左右脚的速度和位移是否超过阈值
            left_moving = any(s > speed_threshold for s in speed_data["left_speed"]) or \
                          any(d > displacement_threshold for d in displacement_data["left_displacement"])
            right_moving = any(s > speed_threshold for s in speed_data["right_speed"]) or \
                           any(d > displacement_threshold for d in displacement_data["right_displacement"])

            return left_moving and right_moving
        numFrame = self.initService(jsonPath, key_filter='all')
        left_ankle, right_ankle = [], []
        neck = []
        right_big_toe, left_big_toe = [], []
        right_knee, left_knee = [], []
        for i in numFrame:
            left_ankle.append(self.getLimbsPosition(7, i))
            right_ankle.append(self.getLimbsPosition(0, i))
            neck.append(self.getLimbsPosition(18, i))
            left_big_toe.append(self.getLimbsPosition(20, i))
            right_big_toe.append(self.getLimbsPosition(21, i))
            left_knee.append(self.getLimbsPosition(13, i))
            right_knee.append(self.getLimbsPosition(14, i))
        # plot_peak(right_big_toe)
        # plot_peak(left_big_toe)
        # plot_peak(left_knee)
        # plot_peak(right_knee)
        motion_data = calculate_speed_and_displacement_with_start_time(left_ankle, right_ankle, 30, neck,  *(left_big_toe, right_big_toe, left_knee, right_knee, neck))
        # 判断是否开始走路
        walking = is_walking(
            speed_data={
                "left_speed": motion_data["left_speed"],
                "right_speed": motion_data["right_speed"]
            },
            displacement_data={
                "left_displacement": motion_data["left_displacement"],
                "right_displacement": motion_data["right_displacement"]
            },
            speed_threshold=0.5,
            displacement_threshold=10
        )
        # 找到上涨点和峰值
        rising_points, peaks = self.find_rising_points(motion_data["a"])

        print("上涨点索引:", rising_points)
        print("峰值点索引:", peaks)

        # 可视化
        import matplotlib.pyplot as plt
        plt.plot(motion_data["a"], label='Data', color='blue')
        plt.scatter(rising_points, [motion_data["a"][i] for i in rising_points], color='green', label='Rising Points')
        plt.scatter(peaks, [motion_data["a"][i] for i in peaks], color='red', label='Peaks')
        plt.legend()
        plt.show()
        plot_peak_1d(motion_data["a"])
        plot_peak_1d(motion_data["left_displacement"])
        plot_peak_1d(motion_data["right_displacement"])
        print("Is the person walking?", walking)
        # print("Motion Data:", motion_data)
if __name__ == '__main__':
    asc = ActionScore()
    jsonPath = 'output/keypoints3333.json'
    with open(jsonPath) as f:
        data = json.load(f)
        asc.action_1_score(data)